#[cfg(feature = "axstd")]
use axstd::println;

use axhal::fcan::*;

pub struct CanFDCtr {
    fcan_system: Option<FCanSystem>,
}

impl CanFDCtr {
    pub fn new() -> Self {
        Self { fcan_system: None }
    }

    pub fn setup(&mut self) {
        let mut fcan_system = FCanSystem::new();

        // 获取并初始化 CAN0 控制器
        let fcan0_controller = fcan_system.controller(0).unwrap();

        match fcan0_controller.init() {
            Ok(_) => println!("CAN0 initialized successfully"),
            Err(_) => {
                println!("Failed to initialize CAN0");
                return;
            }
        }

        fcan0_controller.set_enable(false);
        println!("CAN0 disabled for configuration");
        fcan0_controller.set_canfd_enable(true);
        println!("CAN0 CAN FD enabled");
        fcan0_controller.set_can_mode(1); // 设置为正常模式
        println!("CAN0 set to normal mode");

        println!("Starting CAN loopback test...");
        // 设置仲裁段波特率 1000000 (系统时钟50MHz)
        let arb_cfg = FCanBaudrateConfig {
            segment: FCanSegmentType::Arbitration,
            auto_calc: true,
            baudrate: 1_000_000,

            sample_point: 0,
            prop_seg: 0,   // 由自动计算确定
            phase_seg1: 0, // 由自动计算确定
            phase_seg2: 0, // 由自动计算确定
            sjw: 0,        // 由自动计算确定
            brp: 0,        // 由自动计算确定
        };
        fcan0_controller.set_baudrate(&arb_cfg).ok();

        // 设置数据段波特率 1000000 (系统时钟50MHz)
        let dat_cfg = FCanBaudrateConfig {
            segment: FCanSegmentType::Data,
            auto_calc: true,
            baudrate: 1_000_000,
            sample_point: 0,
            prop_seg: 0,   // 由自动计算确定
            phase_seg1: 0, // 由自动计算确定
            phase_seg2: 0, // 由自动计算确定
            sjw: 0,        // 由自动计算确定
            brp: 0,        // 由自动计算确定
        };
        fcan0_controller.set_baudrate(&dat_cfg).ok();
        println!("CAN0 update  baudrate use calc configured ");

        fcan0_controller.set_enable(false);
        for i in 0..4 {
            let id_mask = FCanIdMaskConfig {
                filter_index: i,
                frame_type: FCanFrameType::Standard,
                id: 0x000,
                mask: 0x1FFFFFFF,
            };
            fcan0_controller.set_id_mask_filter(&id_mask).ok();
        }

        fcan0_controller.enable_id_mask_filter();
        println!("CAN0 configured id mask filter");
        fcan0_controller.set_enable(true);
        println!("CAN0 configured for loopback test");
        println!(
            "CAN0 baudrate set: arbitration {}, data {}",
            arb_cfg.baudrate, dat_cfg.baudrate
        );

        self.fcan_system = Some(fcan_system);
    }

    pub fn write(&mut self, buf: &[u8]) -> usize {
        let fcan0_controller = self.fcan_system.as_mut().unwrap().controller(0).unwrap();

        const FCAN_SEND_ID: u32 = 0x123;
        const FCAN_SEND_ID_FAST: u32 = 0x333; // 快
        const FCAN_SEND_ID_MID: u32 = 0x222; // 中
        const FCAN_SEND_ID_SLOW: u32 = 0x111; // 慢
        const FCAN_SEND_LENGTH: u8 = 16;

        let mut frame = FCanFrame::new();
        // 简化处理 根据首位不同,发送不同的ID的帧数据
        if buf[0] == 1 {
            frame.canid = FCAN_SEND_ID_SLOW;
        } else if buf[0] == 2 {
            frame.canid = FCAN_SEND_ID_MID;
        } else if buf[0] == 3 {
            frame.canid = FCAN_SEND_ID_FAST;
        } else {
            frame.canid = FCAN_SEND_ID;
        }

        frame.canid &= CAN_SFF_MASK; // Standard frame format
        frame.candlc = FCAN_SEND_LENGTH;
        for i in 0..frame.candlc {
            frame.data[i as usize] = i as u8;
        }

        if let Err(e) = fcan0_controller.send_frame(&frame) {
            println!("Failed to send frame: {}", e);
        }
        return 0;
    }

    pub fn read(&mut self, _buf: &mut [u8]) -> usize {
        let fcan0_controller = self.fcan_system.as_mut().unwrap().controller(0).unwrap();
        if let Err(e) = fcan0_controller.receive_frames() {
            println!("Failed to receive frames: {}", e);
        }
        return 0;
    }
}
